#!/home/massage/anaconda3/envs/agv_py/bin/python
#!coding:utf-8

import requests
import json
import time
import socket
import sys

import rospy
from std_msgs.msg import Int64
from std_msgs.msg import Bool



class ApiException(Exception):
    def __init__(self, code, msg):
        self.code = code
        self.msg = msg

    def __repr__(self):
        return "ApiException(%d, %s)" % (self.code, self.msg)

    __str__ = __repr__


# 任务状态
STATE_PENDING = 0
STATE_RUNNING = 1
STATE_WAITING = 2
STATE_ERRWAITING = 3
STATE_CANCELING = 4
STATE_FINISHED = 5
STATE_CANCELED = 6
STATE_ABORTED = 7

# 任务指令
CMD_CANCEL = 2
CMD_VERIFY = 3


class JzAGVApi(object):
    def __init__(self, addr):
        self.addr = addr

    def get_one_robot(self, rid):
        r = requests.get(self.addr + "/api/v1/robots/%d" % rid)
        data = r.json()
        if r.status_code != 200:
            raise ApiException(int(data["code"]), data["msg"])
        return data

    def get_all_robots(self):
        r = requests.get(self.addr + "/api/v1/robots")
        data = r.json()
        if r.status_code != 200:
            raise ApiException(int(data["code"]), data["msg"])
        return data

    def create_mission(self, uuid, map_name, target=0, action=""):
        data = {"ref_uuid": uuid, "steps": [{"map_name": map_name, "target": target, "action": action}], "req_robot": 1}
        r = requests.post(self.addr + "/api/v1/missions", data=json.dumps(data))
        data = r.json()
        if r.status_code != 201:
            raise ApiException(int(data["code"]), data["msg"])
        return data

    def get_one_mission(self, mid):
        r = requests.get(self.addr + "/api/v1/missions/%d" % mid)
        data = r.json()
        if r.status_code != 200:
            raise ApiException(int(data["code"]), data["msg"])
        return data

    def get_one_mission_uuid(self, uuid):
        r = requests.get(self.addr + "/api/v1/uuid/missions/%s" % uuid)
        data = r.json()
        if r.status_code != 200:
            raise ApiException(int(data["code"]), data["msg"])
        return data

    def create_mission_command(self, cmd, mid=None, uuid=None, sync=False):
        data = {"cmd": cmd, "sync": sync}
        if mid is not None:
            data["mission_id"] = int(mid)
        elif uuid is not None:
            data["mission_uuid"] = str(uuid)
        else:
            raise ValueError("mission id and uuid cannot be both None")
        r = requests.post(self.addr + "/api/v1/mscmds", data=json.dumps(data))
        data = r.json()
        if r.status_code != 200 and r.status_code != 201:
            raise ApiException(int(data["code"]), data["msg"])
        return data

    def check_mission_command(self, cid):
        r = requests.get(self.addr + "/api/v1/mscmds/%d" % cid)
        data = r.json()
        if r.status_code != 200:
            raise ApiException(int(data["code"]), data["msg"])
        return data


#定义AGVRobot类用于控制AGV
class AGVRobot:
    def __init__(self,map_name="9y24r",robot_address="192.168.1.102:8858"):
        #初始化机器人
        #param:
        #   map_name: type(str)地图名字
        #   robot_address: type(str)机器人IP地址以及端口号
        #return: None
        self.api=JzAGVApi("http://"+robot_address)
        self.map_name=map_name
        pass
    
    def getNewUuid(self,mission_id):
        #得到机器人mission_id
        uuid = time.strftime("%YN%mY%dR%Hh%Mm%Ss") + 'Mission%d' % mission_id  # missionid
        return (uuid)
    
    def agvMove(self,loadpos):
        #控制机器人到达指定位点
        #param:
        #   loadpos:type(int)机器人指定位点，range:[0,19]
        flag = 0  
        newpos = int(loadpos)
        if newpos <= 0 or newpos > 20:
            print('unexpected point!')
        else:
            print('target', loadpos)
            print(time.time())
            self.agvGuide(newpos)
            print('please wait')
            time.sleep(3)
            
            flag = self.isAGVarrive()
            print(time.time())
            if flag == 1:
                print('AGV arrived')
            else:
                print('sth wrong')
        return flag
      
    def isAGVarrive(self):
        # 判断机器人是否到达指定位点
        rob = self.api.get_one_robot(1)
        time_start=time.time()
        while not rospy.is_shutdown():
            time_end = time.time()
            rob = self.api.get_one_robot(1)
            mission_flag= rob['run_state_str']
            if mission_flag == '已就绪':
                sign = 1
                break
            elif time_end - time_start > 60:
                sign = 0
                break
        return sign
    
    def agvGuide(self,pos):
        # agv实际下发指定函数
        missionid = self.api.get_one_robot(1)['mission_id']
        print('mission', missionid + 1)
        uuid = self.getNewUuid(missionid + 1)
        try:
            print('mission create success :', self.api.create_mission(uuid, self.map_name, target=pos))
        except Exception as e:
            print("create_mission", e)

        try:
            data = self.api.get_one_mission_uuid(uuid)
            print('mission :', data)
        except Exception as e:
            print("get_one_mission_uuid", e)

    
class AgvControl:
    def __init__(self):
        self.agv_index=-1
        #AGV位点到达topic
        self.agv_pub=rospy.Publisher('/agv/agv_arrive', Bool, queue_size=1)
        #指定AGV位点topic
        self.agv_sub=rospy.Subscriber("/agv/agv_position", Int64, self.agvCallback)
        self.agv_robot=AGVRobot()
        print("wait for command")
        rospy.spin()
        print("exit")

    
    def agvCallback(self,msg):
        index=msg.data
        print(index)
        
        flag=self.agv_robot.agvMove(index)
        if flag == 1:
            rospy.sleep(0.1)
        agv_arrive_msg=Bool()
        agv_arrive_msg.data=True
        self.agv_pub.publish(agv_arrive_msg)
        




if __name__ == "__main__":
    rospy.init_node('agv_node')
    agv_control=AgvControl()

